import launch
import launch.event_handlers
import launch.launch_description_sources
import launch_ros
from ament_index_python.packages import get_package_share_directory #获取功能包的资源路径
import os
from launch.substitutions import FindExecutable, PathJoinSubstitution

import launch_ros.parameter_descriptions

def generate_launch_description():   #这个函数名是固定的，不可修改

    # 获取机器人xacro文件路径
    xacro_file = PathJoinSubstitution([
        get_package_share_directory('fishbot_description'),
        'urdf','fishbot',
        'fishbot.urdf.xacro'
    ])

    # 获取世界文件路径
    world_file = PathJoinSubstitution([
        get_package_share_directory('fishbot_description'),
        'world',
        'custom_room.world'
    ])

    # 读取xacro内容
    robot_description_content = launch.substitutions.Command([
        FindExecutable(name='xacro'), ' ', xacro_file              #相当于在命令行 xacro 文件绝对路径生成urdf文件
    ])

    robot_description = launch_ros.parameter_descriptions.ParameterValue(
        robot_description_content,
        value_type=str
    )

    

    #启动节点
    #订阅joint，结合 URDF 中的运动学链，计算每个连杆的坐标系
    action_robot_state_publisher = launch_ros.actions.Node(
        package='robot_state_publisher',         #功能包名字
        executable='robot_state_publisher', #可执行文件名
        parameters=[{'robot_description':robot_description}]
    )

    #启动gazebo并加载世界
    action_launch_gazebo = launch.actions.IncludeLaunchDescription(
        launch.launch_description_sources.PythonLaunchDescriptionSource(
            [get_package_share_directory('gazebo_ros'),'/launch','/gazebo.launch.py']
        ),
        launch_arguments=[('world',world_file),('verbose','true')]
    )

    #使用gazebo的节点去加载机器人模型
    action_spawn_entity = launch_ros.actions.Node(
        package='gazebo_ros',
        executable= 'spawn_entity.py',
        arguments=['-topic','/robot_description','-entity','fishbot']
    )

    #使用命令行激 活题joint_state把 状态接口里的position发布出来，激活之后就可以在rqt，rivz中看见俩个轮子了,可以通过话题查询
    action_load_joint_state_controller = launch.actions.ExecuteProcess(
        cmd='ros2 control load_controller fishbot_joint_state_broadcaster --set-state active'.split(' '),
        output='screen'
    )


    # #激活力控制器
    # action_load_effort_controller = launch.actions.ExecuteProcess(
    #     cmd='ros2 control load_controller fishbot_effort_controller --set-state active'.split(' '),
    #     output='screen'
    # )

    #激活差速控制器
    action_load_diff_driver_controller = launch.actions.ExecuteProcess(
        cmd='ros2 control load_controller fishbot_diff_drive_controller --set-state active'.split(' '),
        output='screen'
    )




    return launch.LaunchDescription([
        #actions动作
        action_robot_state_publisher,
        action_launch_gazebo,
        action_spawn_entity,
        #因为顺序问题，当上一个进程运行后才运行激活任务
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=action_spawn_entity,
                on_exit=[action_load_joint_state_controller],
            )
        ),
        # #激活力控制器
        # launch.actions.RegisterEventHandler(
        #     event_handler=launch.event_handlers.OnProcessExit(
        #         target_action=action_load_joint_state_controller,
        #         on_exit=[action_load_effort_controller],
        #     )
        # ),
        
        #激活差速控制器
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=action_load_joint_state_controller,
                on_exit=[action_load_diff_driver_controller],
            )
        ),
    ])





# #更改后,也不能用
# import launch
# import launch.event_handlers
# import launch.launch_description_sources
# import launch_ros
# from ament_index_python.packages import get_package_share_directory
# import os
# from launch.substitutions import FindExecutable, Command

# def generate_launch_description():

#     # 获取功能包路径
#     fishbot_description_dir = get_package_share_directory('fishbot_description')
#     gazebo_ros_dir = get_package_share_directory('gazebo_ros')

#     # 使用 os.path.join 拼接路径
#     xacro_file = os.path.join(
#         fishbot_description_dir,
#         'urdf',
#         'fishbot',
#         'fishbot.urdf.xacro'
#     )

#     world_file = os.path.join(
#         fishbot_description_dir,
#         'world',
#         'custom_room.world'
#     )

#     gazebo_launch_file = os.path.join(
#         gazebo_ros_dir,
#         'launch',
#         'gazebo.launch.py'
#     )

#     # 读取xacro内容
#     robot_description_content = Command([
#         FindExecutable(name='xacro'), ' ', xacro_file
#     ])

#     robot_description = launch_ros.parameter_descriptions.ParameterValue(
#         robot_description_content,
#         value_type=str
#     )

#     # 启动节点
#     action_robot_state_publisher = launch_ros.actions.Node(
#         package='robot_state_publisher',
#         executable='robot_state_publisher',
#         parameters=[{'robot_description': robot_description}]
#     )

#     # 启动gazebo并加载世界
#     action_launch_gazebo = launch.actions.IncludeLaunchDescription(
#         launch.launch_description_sources.PythonLaunchDescriptionSource(
#             gazebo_launch_file
#         ),
#         launch_arguments={
#             'world': world_file,
#             'verbose': 'true'
#         }.items()
#     )

#     # 加载机器人模型
#     action_spawn_entity = launch_ros.actions.Node(
#         package='gazebo_ros',
#         executable='spawn_entity.py',
#         arguments=['-topic', '/robot_description', '-entity', 'fishbot']
#     )

#     # 控制器命令
#     action_load_joint_state_controller = launch.actions.ExecuteProcess(
#         cmd=['ros2', 'control', 'load_controller', 
#              'fishbot_joint_state_broadcaster', '--set-state', 'active'],
#         output='screen'
#     )

#     action_load_diff_driver_controller = launch.actions.ExecuteProcess(
#         cmd=['ros2', 'control', 'load_controller',
#              'fishbot_diff_drive_controller', '--set-state', 'active'],
#         output='screen'
#     )

#     return launch.LaunchDescription([
#         action_robot_state_publisher,
#         action_launch_gazebo,
#         action_spawn_entity,
#         launch.actions.RegisterEventHandler(
#             event_handler=launch.event_handlers.OnProcessExit(
#                 target_action=action_spawn_entity,
#                 on_exit=[action_load_joint_state_controller],
#             )
#         ),
#         launch.actions.RegisterEventHandler(
#             event_handler=launch.event_handlers.OnProcessExit(
#                 target_action=action_load_joint_state_controller,
#                 on_exit=[action_load_diff_driver_controller],
#             )
#         ),
#     ])